#include "ros_process.h"
#include "pid.h"


ros_imu_data_t ros_imu_data;
ros_odom_data_t ros_odom_data;
ros_speed_data_t ros_speed_data;
ros_distance_data_t ros_distance_data;

ros_motor_input_data_t ros_motor_input_data;
ros_motor_pid_output_data_t ros_motor_pid_output_data;
ros_expect_speed_data_t ros_expect_speed_data;
float ros_voltage_data;

// 创建ros节点
ros::NodeHandle nh;
// 创建发布者
sensor_msgs::Imu imu_msg;
ros::Publisher imu_pub("imu", &imu_msg);
std_msgs::Float32 voltage_msg;
ros::Publisher voltage_pub("voltage", &voltage_msg);
std_msgs::Float32MultiArray speed_msg;
ros::Publisher speed_pub("speed", &speed_msg);
std_msgs::Float32MultiArray distance_msg;
ros::Publisher distance_pub("distance", &distance_msg);
#if DEBUG_PUBLISH
std_msgs::Float32MultiArray expect_speed_msg;
ros::Publisher expect_speed_pub("expect_speed", &expect_speed_msg);
std_msgs::Float32MultiArray motor_input_msg;
ros::Publisher motor_input_pub("motor_input", &motor_input_msg);
std_msgs::Float32MultiArray motor_pid_output_msg;
ros::Publisher motor_pid_output_pub("motor_pid_output", &motor_pid_output_msg);
#endif
nav_msgs::Odometry odom_msg;
ros::Publisher odom_pub("odom", &odom_msg);
// mutex
SemaphoreHandle_t xMutex = NULL;

// 创建订阅者

void host_out_cb(const std_msgs::String& msg)
{
  Serial.println(msg.data);
}
void flag_cb(const std_msgs::Int16MultiArray& msg)
{
  Serial.println(msg.data[0]);
}
void vel_cmd_cb(const geometry_msgs::Twist& msg)
{
  Serial.printf("vx:%f, wz:%f\n", msg.linear.x, msg.angular.z);

  ros_expect_speed_data.exps1 = msg.linear.x * 100.0;
  ros_expect_speed_data.exps2 = msg.linear.x * 100.0;
  ros_expect_speed_data.exps3 = msg.linear.x * 100.0;
  ros_expect_speed_data.exps4 = msg.linear.x * 100.0;
  gyro_pid.setpoint = msg.angular.z;
}
ros::Subscriber<std_msgs::String> sub_host_out("ROS_HOST_OUTPUT", &host_out_cb);

ros::Subscriber<std_msgs::Int16MultiArray> flag_sub("ROS_HOST_INPUT", &flag_cb);
ros::Subscriber<geometry_msgs::Twist> sub_vel_cmd("cmd_vel", &vel_cmd_cb);
// 创建服务
// ros::ServiceServer<std_msgs::Float32> srv("add_two_ints", &add);
// nh.advertiseService(srv);
// 创建客户端
// ros::ServiceClient<std_msgs::Float32> client("add_two_ints", &add);
// nh.serviceClient(client);
// 创建参数
// ros::Param<std_msgs::Float32> param("foo");
// nh.param("foo", param, 0.0);
// 创建时间

void ros_init(char* ip, int port)
{
  // 发布数据
  IPAddress server;
  server.fromString(ip);

  nh.getHardware()->setConnection(server, port);
  nh.initNode();

  // Another way to get IP
  Serial.print("IP = ");
  Serial.println(nh.getHardware()->getLocalIP());
  nh.advertise(imu_pub);
  nh.advertise(voltage_pub);
  nh.advertise(speed_pub);

  nh.advertise(distance_pub);
#if DEBUG_PUBLISH
  nh.advertise(motor_input_pub);
  nh.advertise(motor_pid_output_pub);
  nh.advertise(expect_speed_pub);
  motor_input_msg.data_length = 4;
  motor_input_msg.data = new float[4];
  motor_pid_output_msg.data_length = 4;
  motor_pid_output_msg.data = new float[4];
  expect_speed_msg.data_length = 4;
  expect_speed_msg.data = new float[4];
#endif

  nh.subscribe(sub_host_out);
  nh.subscribe(flag_sub);
  nh.subscribe(sub_vel_cmd);
  nh.advertise(odom_pub);
  // 初始化数据
  imu_msg.header.frame_id = "imu_link";
  imu_msg.header.stamp = nh.now();
  imu_msg.orientation_covariance[0] = -1;
  imu_msg.angular_velocity_covariance[0] = -1;
  imu_msg.linear_acceleration_covariance[0] = -1;
  speed_msg.data_length = 4;
  speed_msg.data = new float[4];
  distance_msg.data_length = 4;
  distance_msg.data = new float[4];
  odom_msg.header.frame_id = "odom";
  odom_msg.child_frame_id = "base_link";
  odom_msg.pose.pose.position.x = 0;
  odom_msg.pose.pose.position.y = 0;
  odom_msg.pose.pose.position.z = 0;
  odom_msg.pose.pose.orientation.x = 0;
  odom_msg.pose.pose.orientation.y = 0;
  odom_msg.pose.pose.orientation.z = 0;
  odom_msg.pose.pose.orientation.w = 1;
  odom_msg.twist.twist.linear.x = 0;
  odom_msg.twist.twist.linear.y = 0;
  odom_msg.twist.twist.linear.z = 0;
  odom_msg.twist.twist.angular.x = 0;
  odom_msg.twist.twist.angular.y = 0;
  odom_msg.twist.twist.angular.z = 0;
  BaseType_t ros_task_t;
  ros_task_t = xTaskCreatePinnedToCore(ros_task, "ros_task", ROS_TASK_STACK_SIZE, NULL, ROS_TASK_PRIORITY, NULL, 1);
  xMutex = xSemaphoreCreateMutex();
  if (ros_task_t != pdPASS)
  {
    Serial.println("Create ros_task failed");
    while (1)
      ;
  }
  else
  {
    Serial.println("Create ros_task success");
  }
}
void ros_publish_voltage()
{
  float voltage = analogRead(Bat_Pin) * 0.0037941156;
  if (voltage < 11.1)
  {
    led_state.flag = LOW_POWER;
  }
  voltage_msg.data = voltage;
  voltage_pub.publish(&voltage_msg);
}
void ros_publish_speed(float speed1, float speed2, float speed3, float speed4)
{
  speed_msg.data[0] = speed1;
  speed_msg.data[1] = speed2;
  speed_msg.data[2] = speed3;
  speed_msg.data[3] = speed4;
  speed_pub.publish(&speed_msg);
}
#if DEBUG_PUBLISH
void ros_publish_expect_speed(float speed1, float speed2, float speed3, float speed4)
{
  expect_speed_msg.data[0] = speed1;
  expect_speed_msg.data[1] = speed2;
  expect_speed_msg.data[2] = speed3;
  expect_speed_msg.data[3] = speed4;
  expect_speed_pub.publish(&expect_speed_msg);
}
void ros_publish_motor_input(float motor_input1, float motor_input2, float motor_input3, float motor_input4)
{
  motor_input_msg.data[0] = motor_input1;
  motor_input_msg.data[1] = motor_input2;
  motor_input_msg.data[2] = motor_input3;
  motor_input_msg.data[3] = motor_input4;
  motor_input_pub.publish(&motor_input_msg);
}
void ros_publish_motor_pid_output(float& motor_pid_output1, float& motor_pid_output2, float& motor_pid_output3,
                                  float& motor_pid_output4)
{
  motor_pid_output_msg.data[0] = motor_pid_output1;
  motor_pid_output_msg.data[1] = motor_pid_output2;
  motor_pid_output_msg.data[2] = motor_pid_output3;
  motor_pid_output_msg.data[3] = motor_pid_output4;
  motor_pid_output_pub.publish(&motor_pid_output_msg);
}
#endif
void ros_publish_distance(float distance1, float distance2, float distance3, float distance4)
{
  distance_msg.data[0] = distance1;
  distance_msg.data[1] = distance2;
  distance_msg.data[2] = distance3;
  distance_msg.data[3] = distance4;
  distance_pub.publish(&distance_msg);
}

void ros_publish_imu(float& ax, float& ay, float& az, float& gx, float& gy, float& gz)
{
  imu_msg.header.stamp = nh.now();
  imu_msg.linear_acceleration.x = ax;
  imu_msg.linear_acceleration.y = ay;
  imu_msg.linear_acceleration.z = az;
  imu_msg.angular_velocity.x = gx;
  imu_msg.angular_velocity.y = gy;
  imu_msg.angular_velocity.z = gz;
  imu_pub.publish(&imu_msg);
}
void ros_publish_odom(float& x, float& y, float& z, float& vx, float& vy, float& vz)
{
  odom_msg.header.stamp = nh.now();
  odom_msg.pose.pose.position.x = x;
  odom_msg.pose.pose.position.y = y;
  odom_msg.pose.pose.position.z = z;
  odom_msg.twist.twist.linear.x = vx;
  odom_msg.twist.twist.linear.y = vy;
  odom_msg.twist.twist.linear.z = vz;
  odom_pub.publish(&odom_msg);
}

void ros_task(void* pvParameters)
{
  static unsigned int imu_count = 0, voltage_count = 12, speed_count = 23, distance_count = 36, motor_input_count = 41,
                      motor_pid_output_count = 53, odom_count = 66, spin_count = 9, expspeed_count = 7;
  uint32_t freq_imu = 100, freq_voltage = 4, freq_speed = 100, freq_distance = 100, freq_motor_input = 100,
           freq_motor_pid_output = 100, freq_odom = 100, spin_freq = 200, expspeed_freq = 100;
  uint32_t period_imu = 500 / freq_imu, period_voltage = 500 / freq_voltage, period_speed = 500 / freq_speed,
           period_distance = 500 / freq_distance, period_motor_input = 500 / freq_motor_input,
           period_motor_pid_output = 500 / freq_motor_pid_output, period_odom = 500 / freq_odom,
           period_expspeed = 500 / expspeed_freq, period_spin = 500 / spin_freq;
  Serial.println("ros_task start");
  delay(1000);
  while (1)
  {
    // Serial.println(2.1);
    if (nh.connected())
    {
      // mutex lock
      // Serial.printf("imu_count:%d, voltage_count:%d, speed_count:%d, distance_count:%d, motor_input_count:%d,
      // motor_pid_output_count:%d, odom_count:%d, spin_count:%d\n",
      //               imu_count, voltage_count, speed_count, distance_count, motor_input_count, motor_pid_output_count,
      //               odom_count, spin_count);
      xSemaphoreTake(xMutex, portMAX_DELAY);

      if (imu_count++ % period_imu == 0)
      {
        ros_publish_imu(ros_imu_data.ax, ros_imu_data.ay, ros_imu_data.az, ros_imu_data.gx, ros_imu_data.gy,
                        ros_imu_data.gz);
      }
      if (voltage_count++ % period_voltage == 0)
      {
        ros_publish_voltage();
      }
      if (speed_count++ % period_speed == 0)
      {
        ros_publish_speed(ros_speed_data.speed1, ros_speed_data.speed2, ros_speed_data.speed3, ros_speed_data.speed4);
      }
      if (distance_count++ % period_distance == 0)
      {
        ros_publish_distance(ros_distance_data.distance1, ros_distance_data.distance2, ros_distance_data.distance3,
                             ros_distance_data.distance4);
      }

      if (odom_count++ % period_odom == 0)
      {
        ros_publish_odom(ros_odom_data.x, ros_odom_data.y, ros_odom_data.z, ros_odom_data.vx, ros_odom_data.vy,
                         ros_odom_data.vz);
      }

#if DEBUG_PUBLISH
      if (expspeed_count++ % period_expspeed == 0)
      {
        ros_publish_expect_speed(ros_expect_speed_data.exps1, ros_expect_speed_data.exps2, ros_expect_speed_data.exps3,
                                 ros_expect_speed_data.exps4);
      }
      if (motor_input_count++ % period_motor_input == 0)
      {
        ros_publish_motor_input(ros_motor_input_data.motor_input1, ros_motor_input_data.motor_input2,
                                ros_motor_input_data.motor_input3, ros_motor_input_data.motor_input4);
      }
      if (motor_pid_output_count++ % period_motor_pid_output == 0)
      {
        ros_publish_motor_pid_output(
            ros_motor_pid_output_data.motor_pid_output1, ros_motor_pid_output_data.motor_pid_output2,
            ros_motor_pid_output_data.motor_pid_output3, ros_motor_pid_output_data.motor_pid_output4);
      }

#endif
      // mutex unlock
      led_state.flag = NORMAL;
    }
    else
    {
      led_state.flag = ROS_NOT_CONNECT;
    }
    if (spin_count++ % period_spin == 0)
    {
      nh.spinOnce();
    }
    xSemaphoreGive(xMutex);
    vTaskDelay(1);
  }
}